#pragma once
#include "ros/ros.h"
#include "control/custom_class.h"
#include "control_msgs/control_msg.h"
#include "amr_trajectory_msgs/amr_trajectory_msg.h"

namespace control
{
	class PathFollowingBase
	{
	public:
		virtual ~PathFollowingBase(){};
		virtual void Init(ros::NodeHandle n) = 0;
		virtual void VehicleController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward, float &max_speed_limit, float &max_angle_speed_limit) = 0;
		virtual void VehiclePoseController(amr_trajectory_msgs::amr_trajectory_msg &trajectory, AMRState &amr_state, float &speed, float &angle_speed, float &speed_toward){};
		
		virtual void VehicleStop()
		{
			speed_ = 0;
			angle_speed_ = 0;
		};

		virtual void ReSet()
		{
			speed_ = 0;
			angle_speed_ = 0;
			pose_control_step_ = 0;
			running_type_ = RunningType::HEADFOLLOWING;
		};
        float speed_;
		float angle_speed_;
		float speed_toward_;
		RunningType running_type_;
		int pose_control_step_;
	private: 
		virtual void LineSpeedLimit(AMRState &amr_state, float &speed, float max_speed_limit) = 0;
	};
} // namespace control